#include "ctrl_protocol.h"
#include "ctrl_bsp_btn.h"
#include "ctrl_bsp_joys.h"
#include "oslib.h"

#include "adc.h"

#include <string.h>
#include <math.h>

/* 控制任务状态 */
typedef enum {
    CTRL_STATE_CONTROL,
    CTRL_STATE_COLIBRATION,
} ctrl_state_t;
static ctrl_state_t ctrl_state = CTRL_STATE_CONTROL;

uint8_t calib_flag = 0;
static uint8_t calib_mask = 0;
typedef enum {
    CALIB_MASK_RY = 0x08,
    CALIB_MASK_RX = 0x04,
    CALIB_MASK_LY = 0x02,
    CALIB_MASK_LX = 0x01,
} calib_mask_t;
uint8_t calib_type  = 0;
typedef enum {
    CALIB_TYPE_NORMAL = 0,
    CALIB_TYPE_THROTTLE = 1,
} calib_type_t;

/**
 * @brief <原始按键数据>到<协议按键数据>的映射, 目前不做处理
 * 
 * @param raw 原始按键数据
 * @return ctrl_button_t 协议按键数据
 */
static ctrl_button_t ctrl_button_mapping(Button_Value_t raw)
{
    return raw;
}

/**
 * @brief 摇杆数据由12位压缩成8位
 * 
 * @param value 12位采样数据
 * @return uint8_t 压缩后的8位数据
 */
static uint8_t ctrl_joystick_12bit_to_8bit(uint16_t value)
{
    return (uint8_t) ((value >> 4) & 0xFF);
}

/**
 * @brief <原始摇杆数据>到<协议摇杆数据>的映射, 完成数据压缩和坐标系的转化
 * 
 * @param raw 原始摇杆数据
 * @return ctrl_joystick_t 协议摇杆数据
 */
static ctrl_joystick_t ctrl_joystick_mapping(ADC_Joystick_Value_t raw)
{
    ctrl_joystick_t ret;

    ret.left_x  = ctrl_joystick_12bit_to_8bit(0xFFF - raw.adc_joy_left_lr) - 0x80;
    ret.left_y  = (calib_type == CALIB_TYPE_THROTTLE) ? ctrl_joystick_12bit_to_8bit(raw.adc_joy_left_fb) 
                                                      : ctrl_joystick_12bit_to_8bit(raw.adc_joy_left_fb) - 0x80;
    ret.right_x = ctrl_joystick_12bit_to_8bit(0xFFF - raw.adc_joy_right_lr) - 0x80;
    ret.right_y = ctrl_joystick_12bit_to_8bit(raw.adc_joy_right_fb) - 0x80;
    
    return ret;
}

/**
 * @brief 将<协议摇杆数据>和<协议按键数据>组成<协议报文>
 * 
 * @param dst 目标地址
 * @param btn_value 协议按键数据
 * @param joys_value 协议摇杆数据
 */
static void ctrl_bsp_to_ctrl_packet(ctrl_prot_t *dst, ctrl_button_t btn_value, ctrl_joystick_t joys_value)
{
    dst->op = (calib_type == CALIB_TYPE_THROTTLE) ? CTRLPROT_OP_DEVICE_CONTROLP : CTRLPROT_OP_DEVICE_CONTROL;
    dst->len = sizeof(ctrl_prot_data_control_t);
    ((ctrl_prot_data_control_t*)(dst->data))->button     = btn_value;
    ((ctrl_prot_data_control_t*)(dst->data))->joystick   = joys_value;
}


#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))

/* 消息发送队列 */
extern osMessageQueueId_t CtrlSendQueueHandle;
/* 显示任务队列 */
extern osMessageQueueId_t CtrlDispQueueHandle;

static ctrl_button_t    btn_value;
static uint8_t          btn_group;
static ctrl_joystick_t  joys_value;
static ctrl_prot_t      packet;
static ADC_Joystick_Value_t joys_smax;
static ADC_Joystick_Value_t joys_smin;
static ADC_Joystick_Value_t joys_err;

/**
 * @brief 控制任务, 负责收集手柄和摇杆数据并初步组成协议报文
 * 
 * @param argument 未使用
 */
void _CtrlTask(void *argument)
{
    osStatus_t ret;
    int i = 0;
    Joystick_Init();
    for (;;) 
    {
        switch(ctrl_state) 
        {
        case CTRL_STATE_CONTROL:
            osDelay(21);
            // 获得按键数据并重新映射到协议报文
            btn_value = ctrl_button_mapping(Button_KeyScan());
            // 获得拨码键盘数据加入到按键数据中, 向显示任务发送对应的数值
            btn_group = Switch_GetState();
            osMessageQueuePut(CtrlDispQueueHandle, &btn_group, NULL, 0);
            ctrl_button_set_group(btn_value, btn_group);
            // 获得摇杆数据并重新映射到协议报文
            joys_value = ctrl_joystick_mapping(Joystick_Sample());
            ctrl_bsp_to_ctrl_packet(&packet, btn_value, joys_value);

            // 向发送任务传递待发送的报文
            ret = osMessageQueuePut(CtrlSendQueueHandle, (uint8_t *)&packet, NULL, 0);
            if (ret == osErrorTimeout) {
                Error("_CtrlTask : Error Timeout");
            }
            else if (ret == osErrorParameter) {
                Fatal("_CtrlTask : Error Parameter");
            }
            else if (ret == osErrorResource) {
                Error("_CtrlTask : Error Resource");
            }
            // 如果需要进入校准状态, 就将状态设置为校准状态
            if (calib_flag) 
            {
                i = 0;
                calib_flag = 0;
                calib_mask = (calib_type == CALIB_TYPE_THROTTLE) ? CALIB_MASK_LY : 0;
                ctrl_state = CTRL_STATE_COLIBRATION;
                memset(&joys_smax, 0xF0, sizeof(joys_smax));
                memset(&joys_smin, 0x7F, sizeof(joys_smin));
            }
            break;
        case CTRL_STATE_COLIBRATION:
            osDelay(20);
            // 采集未纠正偏移的摇杆ADC值
            joys_err = Joystick_Raw_Sample();

            // 得到摇杆ADC值与中心值2047的偏移值
            joys_err.adc_joy_right_fb = 2047 - joys_err.adc_joy_right_fb;
            joys_err.adc_joy_right_lr = 2047 - joys_err.adc_joy_right_lr;
            joys_err.adc_joy_left_fb =  2047 - joys_err.adc_joy_left_fb;
            joys_err.adc_joy_left_lr =  2047 - joys_err.adc_joy_left_lr;

            uprintf("%hd %hd %hd %hd\r\n", joys_err.adc_joy_right_fb, joys_err.adc_joy_right_lr, joys_err.adc_joy_left_fb, joys_err.adc_joy_left_lr);

            // 记录摇杆ADC偏移值的最大值和最小值
            joys_smax.adc_joy_right_fb  = max(joys_err.adc_joy_right_fb, joys_smax.adc_joy_right_fb);
            joys_smax.adc_joy_right_lr  = max(joys_err.adc_joy_right_lr, joys_smax.adc_joy_right_lr);
            joys_smax.adc_joy_left_fb   = max(joys_err.adc_joy_left_fb, joys_smax.adc_joy_left_fb);
            joys_smax.adc_joy_left_lr   = max(joys_err.adc_joy_left_lr, joys_smax.adc_joy_left_lr);
            
            joys_smin.adc_joy_right_fb  = min(joys_err.adc_joy_right_fb, joys_smin.adc_joy_right_fb);
            joys_smin.adc_joy_right_lr  = min(joys_err.adc_joy_right_lr, joys_smin.adc_joy_right_lr);
            joys_smin.adc_joy_left_fb   = min(joys_err.adc_joy_left_fb, joys_smin.adc_joy_left_fb);
            joys_smin.adc_joy_left_lr   = min(joys_err.adc_joy_left_lr, joys_smin.adc_joy_left_lr);
            
            // 采集1000次后, 得到最大偏移值和最小偏移值的平均值, 作为摇杆采集值记录下来
            if (++i >= 1000)
            {
                joys_err.adc_joy_right_fb = (calib_mask & CALIB_MASK_RY) ? 0 : (int16_t)roundf((float)(joys_smax.adc_joy_right_fb + joys_smin.adc_joy_right_fb) / 2.0);
                joys_err.adc_joy_right_lr = (calib_mask & CALIB_MASK_RX) ? 0 : (int16_t)roundf((float)(joys_smax.adc_joy_right_lr + joys_smin.adc_joy_right_lr) / 2.0);
                joys_err.adc_joy_left_fb =  (calib_mask & CALIB_MASK_LY) ? 0 : (int16_t)roundf((float)(joys_smax.adc_joy_left_fb + joys_smin.adc_joy_left_fb) / 2.0);
                joys_err.adc_joy_left_lr =  (calib_mask & CALIB_MASK_LX) ? 0 : (int16_t)roundf((float)(joys_smax.adc_joy_left_lr + joys_smin.adc_joy_left_lr) / 2.0);
                uprintf("Colibration max %hd %hd %hd %hd\r\n", joys_smax.adc_joy_right_fb, joys_smax.adc_joy_right_lr, joys_smax.adc_joy_left_fb, joys_smax.adc_joy_left_lr);
                uprintf("Colibration min %hd %hd %hd %hd\r\n", joys_smin.adc_joy_right_fb, joys_smin.adc_joy_right_lr, joys_smin.adc_joy_left_fb, joys_smin.adc_joy_left_lr);
                uprintf("Colibration err %hd %hd %hd %hd\r\n", joys_err.adc_joy_right_fb, joys_err.adc_joy_right_lr, joys_err.adc_joy_left_fb, joys_err.adc_joy_left_lr);
                if (Joystick_Calibration(joys_err) == 0)
                    uprintf("OK!\r\n");
                else
                    uprintf("Fail!\r\n");
                ctrl_state = CTRL_STATE_CONTROL;
            }
            break;
        default:
            uprintf("Joystick State Error!!!\r\n");
            break;
        }

    }
}